
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       auto_yaw.h
  * @author     baiyang
  * @date       2022-3-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>

#include <common/gp_defines.h>
#include <common/gp_math/gp_mathlib.h>
#include <common/location/location.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// Navigation Yaw control
typedef struct {
    // auto flight mode's yaw mode
    uint8_t _mode;

    // Yaw will point at this location if mode is set to AUTO_YAW_ROI
    Vector3f_t roi;

    // yaw used for YAW_FIXED yaw_mode
    float _fixed_yaw_offset_cd;

    // Deg/s we should turn
    float _fixed_yaw_slewrate_cds;

    // time of the last yaw update
    uint32_t _last_update_ms;

    // heading when in yaw_look_ahead_yaw
    float _look_ahead_yaw;

    // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
    float _yaw_angle_cd;
    float _yaw_rate_cds;
} AutoYaw;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// roi_yaw - returns heading towards location held in roi
float autoyaw_roi_yaw(AutoYaw * autoyaw);

float autoyaw_look_ahead_yaw(AutoYaw * autoyaw);

void autoyaw_set_mode_to_default(AutoYaw * autoyaw, bool rtl);

// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
enum autopilot_yaw_mode autoyaw_default_mode(bool rtl);

// set_mode - sets the yaw mode for auto
void autoyaw_set_mode(AutoYaw * autoyaw, enum autopilot_yaw_mode yaw_mode);

// set_fixed_yaw - sets the yaw look at heading for auto mode
void autoyaw_set_fixed_yaw(AutoYaw * autoyaw, float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle);

// set_fixed_yaw - sets the yaw look at heading for auto mode
void autoyaw_set_yaw_angle_rate(AutoYaw * autoyaw, float yaw_angle_d, float yaw_rate_ds);

// set_roi - sets the yaw to look at roi for auto mode
void autoyaw_set_roi(AutoYaw * autoyaw, const Location* roi_location);

// set auto yaw rate in centi-degrees per second
void autoyaw_set_rate(AutoYaw * autoyaw, float turn_rate_cds);

// yaw - returns target heading depending upon auto_yaw.mode()
float autoyaw_yaw(AutoYaw * autoyaw);

// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
float autoyaw_rate_cds(AutoYaw * autoyaw);

// mode(): current method of determining desired yaw:
static inline enum autopilot_yaw_mode autoyaw_mode(AutoYaw * autoyaw) { return (enum autopilot_yaw_mode)autoyaw->_mode; }

static inline bool autoyaw_fixed_yaw_slew_finished(AutoYaw * autoyaw) { return math_flt_zero(autoyaw->_fixed_yaw_offset_cd); }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



